#include "v4l2api.h"
#include <errno.h>
#include <QDebug>
#include <algorithm>

V4l2Api::V4l2Api(const char* deviceName, QObject *parent)
    : QObject(parent), m_fd(-1), m_isRunning(false)
{
    // 初始化成员数组
    for(int i = 0; i < 4; i++)
    {
        m_mptr[i] = nullptr;
        m_size[i] = 0;
    }

    // [1] 打开设备
    m_fd = open(deviceName, O_RDWR);
    if(m_fd < 0) {
        emit errorOccurred(QString("打开设备失败: %1").arg(strerror(errno)));
        return;
    }

    // [2] 设置采集格式
    struct v4l2_format vfmt;
    memset(&vfmt, 0, sizeof(vfmt));
    vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    vfmt.fmt.pix.width = 640;
    vfmt.fmt.pix.height = 480;
    vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;

    if(ioctl(m_fd, VIDIOC_S_FMT, &vfmt) < 0) {
        emit errorOccurred("设置格式失败");
        ::close(m_fd);
        m_fd = -1;
        return;
    }

    // [3] 申请缓冲区
    struct v4l2_requestbuffers reqbuf;
    memset(&reqbuf, 0, sizeof(reqbuf));
    reqbuf.count = 4;
    reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuf.memory = V4L2_MEMORY_MMAP;

    if(ioctl(m_fd, VIDIOC_REQBUFS, &reqbuf) < 0) {
        emit errorOccurred("申请缓冲区失败");
        ::close(m_fd);
        m_fd = -1;
        return;
    }

    // [4] 内存映射
    struct v4l2_buffer mapbuf;
    memset(&mapbuf, 0, sizeof(mapbuf));
    mapbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    for(int i=0; i<4; i++) {
        mapbuf.index = i;
        if(ioctl(m_fd, VIDIOC_QUERYBUF, &mapbuf) < 0) {
            emit errorOccurred("查询缓冲区失败");
            goto error_cleanup;
        }

        m_mptr[i] = (unsigned char*)mmap(NULL, mapbuf.length,
                                      PROT_READ | PROT_WRITE,
                                      MAP_SHARED, m_fd,
                                      mapbuf.m.offset);
        if(m_mptr[i] == MAP_FAILED) {
            emit errorOccurred("内存映射失败");
            goto error_cleanup;
        }
        m_size[i] = mapbuf.length;

        if(ioctl(m_fd, VIDIOC_QBUF, &mapbuf) < 0) {
            emit errorOccurred("放回缓冲区失败");
            goto error_cleanup;
        }
    }
    return;

error_cleanup:
    for(int j=0; j<4; j++) {
        if(m_mptr[j]) munmap(m_mptr[j], m_size[j]);
    }
    ::close(m_fd);
    m_fd = -1;
}

V4l2Api::~V4l2Api()
{
    stop();
    destory();
    if(m_fd >= 0) ::close(m_fd);
}

bool V4l2Api::start()
{
    if(m_fd < 0) return false;

    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if(ioctl(m_fd, VIDIOC_STREAMON, &type) < 0) {
        emit errorOccurred(QString("开启采集失败: %1").arg(strerror(errno)));
        return false;
    }
    m_isRunning = true;
    return true;
}

void V4l2Api::stop()
{
    if(!m_isRunning || m_fd < 0) return;

    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if(ioctl(m_fd, VIDIOC_STREAMOFF, &type) < 0) {
        emit errorOccurred("停止采集失败");
    }
    m_isRunning = false;
}

void V4l2Api::get_frame(char* buffer, int* size)
{
    if(!m_isRunning || m_fd < 0) {
        *size = 0;
        return;
    }

    struct v4l2_buffer readbuf;
    memset(&readbuf, 0, sizeof(readbuf));
    readbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if(ioctl(m_fd, VIDIOC_DQBUF, &readbuf) < 0) {
        emit errorOccurred("提取数据失败");
        *size = 0;
        return;
    }

    memcpy(buffer, m_mptr[readbuf.index], readbuf.length);
    *size = readbuf.length;

    if(ioctl(m_fd, VIDIOC_QBUF, &readbuf) < 0) {
        emit errorOccurred("放回队列失败");
    }
}

bool V4l2Api::yuv422torgb24(unsigned char *buffer, unsigned char *rgbdata, int w, int h)
{
    for(int i=0; i<w*h/2; i++) {
        unsigned char Y0 = buffer[i*4];
        unsigned char U0 = buffer[i*4+1];
        unsigned char Y1 = buffer[i*4+2];
        unsigned char V1 = buffer[i*4+3];

        // 第一组像素 (Y0, U0, V1)
        int r1 = Y0 + 1.4075*(V1-128);
        int g1 = Y0 - 0.3455*(U0-128) - 0.7169*(V1-128);
        int b1 = Y0 + 1.779*(U0-128);

        // 第二组像素 (Y1, U0, V1)
        int r2 = Y1 + 1.4075*(V1-128);
        int g2 = Y1 - 0.3455*(U0-128) - 0.7169*(V1-128);
        int b2 = Y1 + 1.779*(U0-128);

        // 限制范围并赋值
        rgbdata[i*6]   = qBound(0, r1, 255);
        rgbdata[i*6+1] = qBound(0, g1, 255);
        rgbdata[i*6+2] = qBound(0, b1, 255);
        rgbdata[i*6+3] = qBound(0, r2, 255);
        rgbdata[i*6+4] = qBound(0, g2, 255);
        rgbdata[i*6+5] = qBound(0, b2, 255);
    }
    return true;
}

void V4l2Api::destory()
{
    stop();

    // 取消内存映射
    for(int i = 0; i < 4; i++) {
        if(m_mptr[i]) {
            munmap(m_mptr[i], m_size[i]);
            m_mptr[i] = nullptr;
        }
    }

    // 关闭设备
    if(m_fd >= 0) {
        ::close(m_fd);
        m_fd = -1;
    }
}
